/**
 * @file head.c
 * @brief 云台及发射机构核心功能实现文件
 * @details 包含云台状态更新、控制模式函数（停止/正常/视觉）及发射机构闭环控制逻辑（射速/热量）
 * @author WeiFan Qiu (qiuweifan0523@faxmail.com)
 * @version 1.0
 * @date 2025-08-08
 * 
 * @copyright Copyright (c) 2025  邱伟凡
 * 
 * @par 修改日志:
 *   2025-08-08 初始版本，实现基础控制逻辑
 *	 2025-08-29 与head_task.c合并,gimbal_task和fire_task合并成head_task，加入static关键字
 *
 * 
 * @bug 待解决问题:
 * - 视觉模式下异常处理逻辑待完善
 * - 热量闭环参数需根据实际裁判系统反馈优化
 * - 云台底盘耦合考虑速度环补偿
 */
#include "QWF_Cmd.h"       // 通用命令接口
#include "QWF_Math.h"      // 数学工具库
#include "QWF_Can.h"       // CAN通信头文件
#include "QWF_Usart.h"     // 串口通信头文件
#include "head.h"          // 云台全局结构体定义
#include "state.h"         // 系统状态管理
#include "ctrl.h"          // 控制输入（遥控器/键鼠）
#include "vision.h"        // 视觉模块接口
#include "freertos.h"      // FreeRTOS系统头文件
#include "task.h"          // FreeRTOS任务管理头文件
/**
 * @brief 摩擦轮最小转速阈值（防止堵转）
 * @details 低于此值时视为摩擦轮停止
 */
#define fire_min	100

/**
 * @brief 摩擦轮缓降速/缓加速系数
 * @details 用于控制摩擦轮转速变化的平滑性
 */
#define k_fire_lose	1  // 降速系数
#define k_fire_get	15  // 加速系数

/**
 * @brief 拨盘转速与射速转换系数
 * @details 拨盘转速（rpm）转换为射速（颗/秒）的公式：射速 = 拨盘转速 / 240
 */
#define cooling_to_rpm(rpm)		((rpm)*240)

/**
 * @brief 拨盘最大/最小转速限制
 * @details 根据裁判系统最大热量值动态调整（热量≥140时允许更高转速）
 */
#define knob_v_max	(communicate->chassis.judge.data.str.heat_max>=140?9000:6000)
#define knob_v_min	0  // 最小转速（当前固定为0）

/**
 * @brief 单发所需拨盘角度（编码器单位）
 * @details 拨盘旋转一圈（8192编码器单位）对应4颗弹丸，单发需旋转1/4圈
 */
#define One_bullet_angle		 8192*4

/************************云台核心功能**************************/
/**
 * @brief 更新云台状态（角度/角速度测量值）
 * @details 根据IMU或电机反馈选择数据来源：
 *          - IMU正常时使用IMU数据（精度高）
 *          - IMU异常时使用电机反馈数据（备用方案）
 */
void Gimbal_State_Updata(void) 
{
	if(!(qwf_state->head.gyro.dm_imu.state||qwf_state->head.gyro.hi2xx.state)){
		// IMU正常时，使用陀螺仪数据
		head->part.gimbal.yaw.my.w		=yaw_w_meas_gyro;  // Yaw轴角速度（度/秒）
		head->part.gimbal.yaw.my.angle	=yaw_angle_meas_gyro;  // Yaw轴角度（度）
		head->part.gimbal.pitch.my.w	=pitch_w_meas_gyro;  // Pitch轴角速度（度/秒）
		head->part.gimbal.pitch.my.angle=pitch_angle_meas_gyro;  // Pitch轴角度（度）
	}
	else{
		// IMU异常时，使用电机反馈数据（通过编码器计算）
		head->part.gimbal.yaw.my.w		=-pitch_angle_meas_motor*360/8192.0f;  // Yaw轴角速度（度/秒）
		head->part.gimbal.yaw.my.angle	=-pitch_w_meas_motor*PI/30.0f;  // Yaw轴角度（弧度转度）
		head->part.gimbal.pitch.my.w	=yaw_angle_meas_motor*360/8192.0f;  // Pitch轴角速度（度/秒）
		head->part.gimbal.pitch.my.angle=yaw_w_meas_motor*PI/30.0f;  // Pitch轴角度（弧度转度）
	}	
}
/**
 * @brief Yaw轴负载扭矩计算参数宏定义
 * @details 用于计算Yaw轴电机负载扭矩的线性/二次项系数
 */
#define yaw_k_w		 61.11881468231224   // 角速度一次项系数
#define yaw_k_w2	 -0.5133216412730723  // 角速度二次项系数
#define yaw_k_0		 629.85925140761145   // 扭矩常数项
#define w_yaw_esp	QWF_Esp(head->part.gimbal.yaw.motor.rpm,0)  // Yaw轴电机转速与目标差值
#define yaw_load	QWF_Signed(head->part.gimbal.yaw.motor.rpm,0)*(yaw_k_w*w_yaw_esp+ yaw_k_w2*w_yaw_esp*w_yaw_esp+yaw_k_0)  // Yaw轴负载扭矩计算

/**
 * @brief Pitch轴重力补偿参数宏定义
 * @details 用于计算Pitch轴重力补偿的余弦系数和角度偏移
 */
#define pitch_k_cos		 7531.415736585006f  // 余弦项系数
#define pitch_k_0_angle	 -90.642024258094f    // 角度偏移量（度）
#define pitch_k_0		 -589.8113343720651f   // 重力补偿常数项
#define pitch_g			(pitch_k_cos*arm_cos_f32(PI/180*(pitch_angle_meas_gyro +pitch_k_0_angle))+pitch_k_0)  // Pitch轴重力补偿值计算

/**
 * @brief Yaw轴角度补偿参数宏定义
 * @details 用于计算Yaw轴角度补偿的余弦系数和角度偏移
 */
#define yaw_angle_k_cos		 1348.0715284718692f  // 余弦项系数
#define yaw_angle_k_0_angle	 -34.220357477444765f  // 角度偏移量（度）
#define yaw_angle_k_0		 -357.5365380508824f   // 角度补偿常数项
#define yaw_angle_g			(yaw_angle_k_cos*arm_cos_f32(PI/180*(yaw_angle_meas_motor*180/8192.0f +yaw_angle_k_0_angle))+yaw_angle_k_0)  // Yaw轴角度补偿值计算

/**
 * @brief 云台控制任务（1ms周期）
 * @details 负责云台Yaw/Pitch轴的角度/角速度PID控制计算、电机数据发送及调试数据上传
 * @param argument FreeRTOS任务参数（未使用）
 */ 
float chassis_top_como=0.017f;  // 底盘与云台Yaw轴耦合系数
void head_task(void const * argument){
	static TickType_t xLastWakeTime;
    const TickType_t xPeriod = pdMS_TO_TICKS(1); // 任务周期1ms
    xLastWakeTime = xTaskGetTickCount();  // 初始化上次唤醒时间
	head->pid=head_pid;
	while(1){
		Gimbal_State_Updata();  // 更新云台状态（如传感器数据、电机反馈等）
		head->part.gimbal.ctrl_func[gimbal_mode!=gimbal_stop]();  // 非停止模式开启pid
		head->part.shoot.ctrl_func [shoot_mode !=fire_stop  ]();  // 非停止模式开启pid
		if(ctrl->type!=Null_Ctrl){  // 存在有效控制输入时
			head->part.gimbal.ctrl_func[gimbal_mode[now]]();  // 执行当前云台模式控制函数
			if(ctrl->mode==RC){  // 识别遥控器控制模式
				head->part.shoot.ctrl_func[shoot_mode[now]]();  // 执行当前发射模式控制函数
			}
			else{  // 键鼠控制模式
				if(ctrl->KM[now].mouse.press_l)  // 鼠标左键按下
					head->part.shoot.ctrl_func[shoot_mode[now]]();  // 执行当前发射模式控制函数
				else  // 鼠标左键未按下
					head->part.shoot.ctrl_func[fire_knob_stop]();  // 切换到拨弹停止模式
			}		
		}
		else{  // 无控制输入时
			head->part.gimbal.ctrl_func[gimbal_mode[now]=gimbal_stop]();  // 切换到云台停止模式
			head->part.shoot.ctrl_func[shoot_mode[now]=fire_stop]();  	 // 切换到发射停止模式			
		}
		//模式更替
		gimbal_mode[last]=gimbal_mode[now];
		shoot_mode[last]=shoot_mode[now];		
		/***************************************Pitch轴控制***************************************************************/
		// 角度误差小于2.5度时启用前馈控制（原注释被优化，因为pitch开启前馈电机会乱叫）
//		if(QWF_Esp(pitch_angle_target,pitch_angle_meas)<=2.5f)
//			head_pid->gimbal.pitch.angle.mode.ffc=1;
//		else
			head_pid->gimbal.pitch.angle.mode.ffc=0;
		// 角度环PID计算（输出目标角速度）
		pitch_w_target=PID_Calculate(&head_pid->gimbal.pitch.angle,pitch_angle_meas,pitch_angle_target);

		#if use_pitch_g_add==QWF_Enable  // 启用重力补偿时
//		head_pid->gimbal.pitch.w.K0=pitch_g;  // 设置角速度环自定义项为重力补偿值
		#endif
		// 角速度环PID计算（输出电机电流目标值）
		head->part.gimbal.pitch.target.out=(int16_t)PID_Calculate(&head_pid->gimbal.pitch.w,pitch_w_meas,pitch_w_target);
		
		/****************************************Yaw轴控制****************************************************************/
		if(QWF_Esp(head_pid->gimbal.yaw.angle.target[now],yaw_angle_target)<=0.3f){  // 角度误差小于0.3度时启用前馈控制
			head_pid->gimbal.yaw.angle.mode.ffc=1;
		}else{
			head_pid->gimbal.yaw.angle.mode.ffc=0;
		}

		// 角度环PID计算（考虑底盘耦合补偿(云台电机角速度-陀螺仪角速度（转化成同单位）=底盘角速度，底盘角速度*云台底盘耦合系数=耦合补偿值)）
		yaw_w_target=PID_Calculate(&head_pid->gimbal.yaw.angle,
						find_min_angle(yaw_angle_meas-(head->part.gimbal.yaw.motor.rx_meas.rpm-yaw_w_meas*0.5f/3)*chassis_top_como,yaw_angle_target,360),
									yaw_angle_target);

		// 角速度环PID计算（输出电机电流目标值）
		head->part.gimbal.yaw.target.out=(int16_t)PID_Calculate(&head_pid->gimbal.yaw.w,
									yaw_w_meas,yaw_w_target);
		/***************************摩擦轮控制*******************************************/
		// 左摩擦轮速度环PID计算（输出电机电流目标值）
		head->part.shoot.fire.l.target.out=(int16_t)PID_Calculate(&head_pid->shoot.fire.l.v,	fire_l_v_meas,	 fire_l_v_target);
		// 右摩擦轮速度环PID计算（输出电机电流目标值）
		head->part.shoot.fire.r.target.out=(int16_t)PID_Calculate(&head_pid->shoot.fire.r.v,	fire_r_v_meas,   fire_r_v_target);
		/*电机控制数据发送*/
		send_data_head_gm6020(pitch);  // 发送Pitch轴电机控制数据（GM6020电机）
		send_data_head_gm6020(yaw);   // 发送Yaw轴电机控制数据（GM6020电机）		
		send_data_head_c620(knob);    // 发送拨弹电机控制数据（C620电调）
		send_data_head_c620(fire.l);  // 发送左摩擦轮电机控制数据（C620电调）
		send_data_head_c620(fire.r);  // 发送右摩擦轮电机控制数据（C620电调）
		/*调试数据上传VOFA+*/
		Add_Data_To_Vofa(vision->rx.header,vision_head);  // 上传视觉数据
		// Pitch轴调试数据（角度/速度/电流）
		Add_Data_To_Vofa(head_pid->gimbal.pitch.angle.target[now]			,pitch_angle_tar);  // 目标角度
		Add_Data_To_Vofa(head_pid->gimbal.pitch.angle.measure[now]			,pitch_angle_meas);  // 实际角度
		Add_Data_To_Vofa(head_pid->gimbal.pitch.w.target[now]				,pitch_v_tar);      // 目标速度
		Add_Data_To_Vofa(head_pid->gimbal.pitch.w.measure[now]				,pitch_v_meas);     // 实际速度
		Add_Data_To_Vofa(head->part.gimbal.pitch.target.out					,pitch_i_tar);      // 目标电流
		Add_Data_To_Vofa(head->part.gimbal.pitch.motor.rx_meas.torque		,pitch_i_meas);     // 实际电流

		// Yaw轴调试数据（角度/速度/电流）
		Add_Data_To_Vofa(head_pid->gimbal.yaw.angle.target[now]				,yaw_angle_tar);    // 目标角度
		Add_Data_To_Vofa(head_pid->gimbal.yaw.angle.measure[now]			,yaw_angle_meas);   // 实际角度
		Add_Data_To_Vofa(head_pid->gimbal.yaw.w.target[now]					,yaw_v_tar);        // 目标速度
		Add_Data_To_Vofa(head_pid->gimbal.yaw.w.measure[now]				,yaw_v_meas);       // 实际速度
		Add_Data_To_Vofa(head->part.gimbal.yaw.target.out					,yaw_i_tar);        // 目标电流
		Add_Data_To_Vofa(head->part.gimbal.yaw.motor.rx_meas.torque			,yaw_i_meas);       // 实际电流
//		Add_Data_To_Vofa(yaw_angle_g		,yaw_angle_g);  // Yaw轴角度补偿值（调试用）
		//摩擦轮调试数据
		Add_Data_To_Vofa(fire_l_v_target,fire_l_v_target);  // 左摩擦轮目标速度
		Add_Data_To_Vofa(fire_l_v_meas,fire_l_v_meas);    // 左摩擦轮实际速度
		Add_Data_To_Vofa(fire_r_v_target,fire_r_v_target);  // 右摩擦轮目标速度
		Add_Data_To_Vofa(fire_r_v_meas,fire_r_v_meas);    // 右摩擦轮实际速度

		// 拨弹电机调试数据（速度）
		Add_Data_To_Vofa(head_pid->shoot.knob.w.measure[now],	knob_meas);  // 拨弹电机实际速度
		Add_Data_To_Vofa(head_pid->shoot.knob.w.target[now],	knob_tar);   // 拨弹电机目标速度
		Add_Data_To_Vofa(head_pid->shoot.knob.w.output,			knob_out);   // 拨弹电机控制输出
		vTaskDelayUntil(&xLastWakeTime, xPeriod);  // 任务延时（严格周期执行）
	}
}
/**
 * @brief 云台卸力控制（停止PID输出）
 * @details 禁用角度环和角速度环输出，保持当前角度
 */
static void Gimbal_Stop(void){
	// 禁用所有PID环输出
	head_pid->gimbal.yaw.angle   	.mode.out_put=0;
	head_pid->gimbal.yaw.w    		.mode.out_put=0;	
	head_pid->gimbal.pitch.angle	.mode.out_put=0;
	head_pid->gimbal.pitch.w    	.mode.out_put=0;
	// 目标角度锁定为当前角度
	yaw_angle_target=yaw_angle_meas;
	pitch_angle_target=pitch_angle_meas;
}

/**
 * @brief 云台启用控制（恢复PID输出）
 * @details 启用角度环和角速度环输出
 */
static void Gimbal_Yes(void){
	// 启用所有PID环输出
	head_pid->gimbal.yaw.angle   	.mode.out_put=1;
	head_pid->gimbal.yaw.w    		.mode.out_put=1;	
	head_pid->gimbal.pitch.angle	.mode.out_put=1;
	head_pid->gimbal.pitch.w    	.mode.out_put=1;	
}

/**
 * @brief 云台正常控制模式
 * @details 根据遥控器或键鼠输入调整目标角度
 * @note 遥控器输入范围：-660~+660（对应通道值）
 */
static void Gimbal_Normal(void){
	if( ctrl->mode==RC){
		// 遥控器控制：通过左摇杆左右/上下调整Yaw/Pitch角度
		yaw_angle_target=yaw_angle_target-ctrl->RC[now].ch[0]/660.0f/2.0f;  // Yaw轴角度调整（摇杆左/右对应角度减/增）
		pitch_angle_target=QWF_Limiter(pitch_angle_target+ctrl->RC[now].ch[1]/660.0f/2.0f,PITCH_MAX,PITCH_MIN);  // Pitch轴角度限幅（-15°~30°）
	}
	else if (ctrl->mode==KM){
		// 键鼠控制：通过鼠标移动调整角度（需配合灵敏度系数KM_Yaw_factor/KM_Pitch_factor）
		yaw_angle_target=find_min_angle(yaw_angle_target-ctrl->KM[now].mouse.x*KM_Yaw_factor,yaw_angle_meas,360);  // Yaw轴最小角度差调整
		pitch_angle_target=QWF_Limiter(pitch_angle_target+ctrl->KM[now].mouse.y*KM_Pitch_factor,PITCH_MAX,PITCH_MIN);  // Pitch轴角度限幅
	}
	return;
}

/**
 * @brief 云台视觉跟随模式
 * @details 优先使用视觉反馈调整角度，异常时切换至正常控制
 */
static void Gimbal_Vision(void){
	if(!qwf_state->head.vision.state&&vision->rx.distance>0)  // 视觉模块正常且目标距离有效
		Vision_Control();  // 执行视觉控制逻辑
	else
		head->part.gimbal.ctrl_func[gimbal_normal]();  // 异常时切换至正常控制模式
//RV_Vision_Control();
	return;
}

/**
 * @brief 云台视觉测试模式
 * @details 生成正弦曲线目标角度，用于测试云台跟随性能
 */
static void Gimbal_Vision_Test(void){
	static uint32_t tick=0;
	tick++;
	// 生成正弦曲线目标角度（幅度20°，频率1Hz）
	yaw_angle_target=20.0f*arm_sin_f32(10.0f*PI*(tick-220000)/10000.0f);
	pitch_angle_target=10.0f*arm_sin_f32(10.0f*PI*(tick-220000)/10000.0f);
	if(tick>=2200000)  // 周期重置
		tick=0;
	return;
}

/*************************发射机构闭环控制***************************/
#include "communicate.h"  // 包含裁判系统数据结构体

/**
 * @brief 发射机构闭环控制（射速/热量）
 * @details 根据裁判系统反馈调整摩擦轮和拨盘的最大转速
 */
static void shoot_loop(void){
	static float last_fire_speed_diff=0;  // 上一周期射速差值（判断是否发弹）
	static bool shoot_bullet=0;  // 发弹标志位

	// 检测是否发弹（射速差值变化且差值在合理范围）
	if((my_fire_speed_diff!=last_fire_speed_diff)&&QWF_Esp(last_fire_speed_diff,my_fire_speed_diff)<=10){
		shoot_bullet=1;
	}
	else{
		shoot_bullet=0;
	}

	// 射速闭环控制（摩擦轮转速限制）
	if(head->part.shoot.fire.speed_loop.use){
		if(shoot_bullet){
			if(my_fire_speed_diff>=2){
				// 射速过大时紧急降速
				head->part.shoot.fire.speed_loop.v_max-=1000;
				head->part.shoot.fire.speed_loop.over_num++;
			}
			else if(QWF_Esp(my_fire_speed_diff,0)>=initial_speed_tar_range){
				// 射速超过阈值时调整最大转速
				head->part.shoot.fire.speed_loop.v_max-=QWF_Signed(my_fire_speed_diff,0)*my_speed_add;
			}
			// 限制最大转速（防止失控）
			if(head->part.shoot.fire.speed_loop.v_max>=fire_max)
				head->part.shoot.fire.speed_loop.v_max=fire_max;
		}
	}
	else{
		// 未启用射速闭环时，默认最大转速6500rpm
		head->part.shoot.fire.speed_loop.v_max=6500;
	}
	// 更新摩擦轮斜坡控制最大值
	head->part.shoot.fire.ramp.max=head->part.shoot.fire.speed_loop.v_max;
/*热量控制思路heat_max=最大热量-冷却值
   knob_v
	。
   /|\
	|v_max
	|----------- \			v=k*(heat-x0)+v0---->k=(v_min-v_max)/(heat_max*0.5f)	x0=heat_max*0.25f,v0=v_max	
	|        :      \
	|        :         \
	|        :            \     v_min
	|        :               \----------------
	|		 :				 :			       \
	。------------------------------------------。 heat
	   heat_max*0.25f     heat_max*0.75f       /
	实测需要v_max不能过大，不然冷却优先四级前会超（速度转化成冷却比裁判系统反馈频率还快了），爆发优先不影响，冷却优先改一下v_max成裁判系统反馈频率对应的
	裁判系统热量反馈10hz，即1秒10次/0.1s一次，在这一次射出的颗数-这一次冷却的颗数不可超过最大热量值
	但是实际上不仅要关注裁判系统周期，还要关注控制周期，两者合起来为控制延时假设为0.37s
	方程：设这一次的冷却值为c（颗/s），最大热量值为h(颗)，控制延时为t(s)，射速为v（颗/s）
			（v-c）*t<=h
				v<=h/t+c
			变成转速*240->v=240*(h/t+c)
			拨盘转速最大9000,即37.5颗/s，哪怕没冷却，也只要140热量就达到了，所以只要限制<140，即冷却优先前4级为6000
	*/
//	#define heat_max_with_cooling			(communicate->chassis.judge.data.str.heat_max)
	// 热量闭环控制（拨盘转速限制）
#define heat_max_with_cooling	(communicate->chassis.judge.data.str.heat_max+communicate->chassis.judge.data.str.heat_cooling*0.1)
	if(head->part.shoot.knob.heat_loop.use&&ctrl->mode!=RC){
		if(communicate->chassis.judge.data.str.heat_now>communicate->chassis.judge.data.str.heat_max){
			// 热量超限时紧急停拨盘
			if(shoot_bullet){
				head->part.shoot.knob.heat_loop.over_num++;
				head->part.shoot.knob.heat_loop.v_max=0;
			}
		}
		else if(communicate->chassis.judge.data.str.heat_now<=heat_max_with_cooling*0.5f){
			// 热量较低时允许最大转速
			head->part.shoot.knob.heat_loop.v_max=knob_v_max;
		}			
		else if(communicate->chassis.judge.data.str.heat_now<=heat_max_with_cooling*0.75f){
			// 热量中等时线性降低转速
			head->part.shoot.knob.heat_loop.v_max=(knob_v_min-knob_v_max)/(heat_max_with_cooling*0.5f)*(communicate->chassis.judge.data.str.heat_now-heat_max_with_cooling*0.25f)+knob_v_max;		
		}
		else if(communicate->chassis.judge.data.str.heat_now<=heat_max_with_cooling*1.0f){
			// 热量接近上限时限制最小转速
			head->part.shoot.knob.heat_loop.v_max=knob_v_min;
		}		
		else{
			// 热量严重超限时停拨盘
			head->part.shoot.knob.heat_loop.v_max=0;
		}
	}
	else{
		// 未启用热量闭环时，默认最大转速6000rpm
		head->part.shoot.knob.heat_loop.v_max=6000;
	}
	// 记录当前射速差值
	last_fire_speed_diff=my_fire_speed_diff;	
}

/**
 * @brief 停止发射（摩擦轮缓降速）
 * @details 摩擦轮转速缓慢降至最小值，防止急停导致卡弹
 */
static void Fire_Stop(void){
	// 摩擦轮缓降速（目标值：fire_min，当前值：ramp.now，降速系数：k_fire_lose）
	head->part.shoot.fire.ramp.now=QWF_Ramp(fire_min,	head->part.shoot.fire.ramp.now,	 k_fire_lose);
	// 设置摩擦轮状态为未就绪
	head->part.shoot.fire.state=unready;
	// 左右摩擦轮目标转速（方向相反）
	fire_l_v_target=head->part.shoot.fire.ramp.now;
	fire_r_v_target=-head->part.shoot.fire.ramp.now;
	// 当转速降至阈值以下时禁用PID输出
	if((QWF_Esp(fire_r_v_meas,0)<=fire_min)&&(QWF_Esp(fire_l_v_meas,0)<=fire_min))
		head_pid->shoot.fire.r.v.mode.out_put=head_pid->shoot.fire.l.v.mode.out_put=0;
	// 重置单发标志位

	head_pid->shoot.knob.angle.mode.out_put=0;
	head_pid->shoot.knob.w.mode.out_put=0;
	head_pid->shoot.knob.w_once.mode.out_put=0;//以防后续函数用pid给拨盘输出，pid输出模式关闭
	head->part.shoot.knob.target.out=0;//拨盘输出置0（因为拨盘输出单独拎出来了）
	return;
}
/**
 * @brief 正常发弹控制函数
 * @details 启用拨盘和摩擦轮PID环，通过斜坡控制逐步提升摩擦轮转速，判断摩擦轮准备状态
 */
static void Fire_Yes(void){//发射
	// 启用拨盘角度环、角速度环及左右摩擦轮速度环输出

    head_pid->shoot.fire.l.v    .mode.out_put=1;    // 左摩擦轮速度环输出使能
    head_pid->shoot.fire.r.v    .mode.out_put=1;    // 右摩擦轮速度环输出使能

	shoot_loop();
	// 摩擦轮斜坡控制（逐步提升转速，避免电机烧坏掉（无人机应该尤其注意））
	head->part.shoot.fire.ramp.now=QWF_Ramp(head->part.shoot.fire.ramp.max,head->part.shoot.fire.ramp.now,k_fire_get);               
	if(head->part.shoot.fire.ramp.now>head->part.shoot.fire.ramp.max)
		head->part.shoot.fire.ramp.now=head->part.shoot.fire.ramp.max;
	
	// 设置左右摩擦轮目标转速（左右反向，其他车可能符号是反的，具体看机械装配）
	fire_l_v_target=1*head->part.shoot.fire.ramp.now;
	fire_r_v_target=-1*head->part.shoot.fire.ramp.now;
	// 判断摩擦轮是否准备好（当前转速达到目标的一定比例）
	if(QWF_Esp(fire_l_v_meas, 0) >= head->part.shoot.fire.ramp.max * fire_persent_to_knob &&
       QWF_Esp(fire_r_v_meas, 0) >= head->part.shoot.fire.ramp.max * fire_persent_to_knob){
        head->part.shoot.fire.state = ready;  // 标记摩擦轮准备完成
    }
    head_pid->shoot.knob.w      .mode.out_put=1;    // 拨盘连发角速度环输出使能
    head_pid->shoot.knob.angle  .mode.out_put=1;    // 拨盘角度环输出使能
	head_pid->shoot.knob.w_once	.mode.out_put=1;	// 拨盘单发速度环输出使能
	return;
}
/**
 * @brief 停止拨盘但保持摩擦轮运行
 * @details 停止拨盘电机，标记单发状态，逐步降低拨盘转速至0
 */
static void Fire_Knob_Stop(void){//不发射,但是摩擦轮还动
	knob_v_target=0;// 拨盘目标转速设为0
	head->part.shoot.knob.target.out=PID_Calculate(&head_pid->shoot.knob.w_once,				knob_v_meas,  		knob_v_target	);
    // 当拨盘转速降至200rpm以下时，关闭所有拨盘控制环（防止超调反转损坏拨盘;ps:半舵拨盘是没发反转的）	
	if(knob_v_meas<=200){
		head_pid->shoot.knob.angle.mode.out_put=0;
		head_pid->shoot.knob.w.mode.out_put=0;
		head_pid->shoot.knob.w_once.mode.out_put=0;
		head->part.shoot.knob.target.out=0;
	}
	head->part.shoot.fire.state=unready; // 标记摩擦轮未准备好
	return;
}
/**
 * @brief 单发控制函数（不限制热量）
 * @note  单发不限制热量，应对场上紧急情况
 * @details 根据条件触发单次拨盘转动，推送一发弹丸
 */
static void Fire_Once(void){
	head->part.shoot.knob.motor.rx_meas.flag=with_round;
	if(head->part.shoot.fire.state/*摩擦轮一定要准备好，不然一定不发射*/&&\
	  (		(vision->rx.fire_advice&&gimbal_mode[now]==gimbal_vision&&\
				(vision_mode==vision_mode_small_flag||vision_mode_big_flag)/*在视觉打幅模式（用火控的情况下）*/)||\
			(!vision->rx.use_fire_advice/*不用火控*/)||\
			(gimbal_mode[now]!=gimbal_vision/*不在视觉模式*/))){
		if(shoot_mode[now]!=shoot_mode[last]){// 仅当未触发过单次发射时执行
			// 设置拨盘目标角度（当前角度 + 单发所需角度）(最多到meas;326510)
			knob_angle_target=knob_angle_meas+One_bullet_angle;
			if(knob_angle_target>=8192*19){// 防止角度溢出（超过拨盘最大机械角度时归零圈数）
					head->part.shoot.knob.motor.rx_meas.round-=(int)knob_angle_target/(19*8192)*19;
					knob_angle_target=fmod(knob_angle_target,19*8192);
			}
	}
		//双环控制
		knob_v_target=PID_Calculate(&head_pid->shoot.knob.angle,			knob_angle_meas,	knob_angle_target);
		head->part.shoot.knob.target.out=PID_Calculate(&head_pid->shoot.knob.w_once,				knob_v_meas,  		knob_v_target	);
	}
	else{//其余状况,不用急停(控制速度为0)，因为发完一发就会立马固定角度值
		head->part.shoot.knob.target.out=0;		
	}
	return;
}

/**
 * @brief 连发控制函数（限制热量）
 * @note  连发限制热量，以防超热量
 * @details 根据条件持续驱动拨盘，实现连续发射
 */
static void Fire_Continuous(void){
	if(head->part.shoot.fire.state/*摩擦轮一定要准备好，不然一定不发射*/&&\
	  (		(vision->rx.fire_advice&&gimbal_mode[now]==gimbal_vision/*在视觉模式（用火控）*/)||\
			(!vision->rx.use_fire_advice/*不用火控*/)||\
			(gimbal_mode[now]!=gimbal_vision/*不在视觉模式*/))){
				
		knob_v_target=head->part.shoot.knob.heat_loop.v_max; // 使用热量闭环限制的最大转速
		head->part.shoot.knob.target.out=PID_Calculate(&head_pid->shoot.knob.w,		knob_v_meas,  	 knob_v_target	)	;
	}
	else if(vision->rx.fire_advice==0&&gimbal_mode[now]==gimbal_vision){//自瞄火控不让开火
		knob_v_target=0; //拨盘转速设为0
		head->part.shoot.knob.target.out=PID_Calculate(&head_pid->shoot.knob.w,		knob_v_meas,  	 knob_v_target	)	;		
	}
	else{//其余状况
		head->part.shoot.knob.target.out=0;		
	}
	
	return;
}
/**
 * @brief 云台与发射机构控制结构体实例化
 */
static head_t Head={
	.part={
		.gimbal={
			.ctrl_func={Gimbal_Stop,Gimbal_Yes,Gimbal_Normal,Gimbal_Vision,Gimbal_Vision_Test},		
			.mode=gimbal_stop, // 默认云台停止模式
			motor_str_config(pitch.motor,pitch	,rx_and_tx)
			motor_str_config(yaw.motor	,yaw	,rx_and_tx)
			},
		.shoot={
			.ctrl_func={Fire_Stop,Fire_Yes,Fire_Knob_Stop,Fire_Once,Fire_Continuous},
			.mode=fire_stop,// 默认发射停止模式
			motor_str_config(fire.l.motor,fire_l,rx_and_tx)
			motor_str_config(fire.r.motor,fire_r,rx_and_tx)
			motor_str_config(knob.motor	 ,knob  ,rx_and_tx)	
			.knob.heat_loop={.use=1,.v_max=6500},
			.fire.speed_loop.use=1,
			.fire.speed_loop.v_max=6000,
			.fire.ramp.max=6000,
		},
	},
};
head_t* head=&Head;
